﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace UltrasonicTDPlatform
{
    class MyAlgorithm
    {
        public static void CurrentFilter(double[] current, double[] voltage, int len, double k)
        {
            // 计算电压差分，用来补偿电流脉冲干扰

            double[] voltage_dif = new double[len];
            for (int i = 1; i < len; i++)
            {
                voltage_dif[i] = voltage[i] - voltage[i - 1];
                current[i] = current[i] - k * voltage_dif[i];
            }

            // 中值滤波
            for (int i = 1; i < len-1; i++)
            {
                double[] temp = new double[3];
                temp[0] = current[i - 1];
                temp[1] = current[i];
                temp[2] = current[i + 1];

                Array.Sort(temp);
                current[i] = temp[1];
            }


        }

       
    }

    class FFT
    {
        int n, m;
        // Lookup tables. Only need to recompute when size of FFT changes.
        double[] cos;
        double[] sin;

        public FFT(int n)
        {
            this.n = n;
            this.m = (int)(Math.Log(n) / Math.Log(2));

            // Make sure n is a power of 2
            if (n != (1 << m))
                Console.Out.WriteLine("FFT length must be power of 2");

            // precompute tables
            cos = new double[n / 2];
            sin = new double[n / 2];

            for (int i = 0; i < n / 2; i++)
            {
                cos[i] = Math.Cos(-2 * Math.PI * i / n);
                sin[i] = Math.Sin(-2 * Math.PI * i / n);
            }

        }
        ///x为实部y为虚部///
        public void fft(double[] x, double[] y)
        {
            int i, j, k, n1, n2, a;
            double c, s, t1, t2;

            // Bit-reverse
            j = 0;
            n2 = n / 2;
            for (i = 1; i < n - 1; i++)
            {
                n1 = n2;
                while (j >= n1)
                {
                    j = j - n1;
                    n1 = n1 / 2;
                }
                j = j + n1;

                if (i < j)
                {
                    t1 = x[i];
                    x[i] = x[j];
                    x[j] = t1;
                    t1 = y[i];
                    y[i] = y[j];
                    y[j] = t1;
                }
            }

            // FFT
            n1 = 0;
            n2 = 1;

            for (i = 0; i < m; i++)
            {
                n1 = n2;
                n2 = n2 + n2;
                a = 0;

                for (j = 0; j < n1; j++)
                {
                    c = cos[a];
                    s = sin[a];
                    a += 1 << (m - i - 1);

                    for (k = j; k < n; k = k + n2)
                    {
                        t1 = c * x[k + n1] - s * y[k + n1];
                        t2 = s * x[k + n1] + c * y[k + n1];
                        x[k + n1] = x[k] - t1;
                        y[k + n1] = y[k] - t2;
                        x[k] = x[k] + t1;
                        y[k] = y[k] + t2;
                    }
                }
            }
        }


    }
}
